#include "myserialpreview.h"
#include "../../common/appenv.h"
#include "../../common/wyq_log.h"
#include <datasource/common/videodev2.h>
#include <QDebug>
#include <QDataStream>
#include <datasource/wy6005.h>
#include <datasource/common/wy_log.h>

#define MYDebug() do{qDebug() << __FILE__ << "{" << __func__ << ":" << __LINE__ << "}";}while(0)
static int get_data_len(char dlc);

MySerialPreview::MySerialPreview():
    is_working(false)
{
    currentData.frame = -1;
    currentData.block_count = 0;
    memset(currentData.rows, 0, sizeof(currentData.rows));
    memset(currentData.blocks, 0, sizeof(currentData.blocks));
    memset(currentData.rawdata, 0, sizeof(currentData.rawdata));
    cloud_ptr = std::make_shared<open3d::geometry::PointCloud>();
}

bool MySerialPreview::consumerInit()
{
    // Data processing interface configuration
    int width = WY6005_WIDTH;
    int height = WY6005_HEIGHT;

    int devid = 999999;
    cxx_camera = new EmptyCamera(AppEnv::appPath().toLocal8Bit().constData(), devid);

    int ret = cxx_camera->InitCamera(width, height);
    if (ret != 0) {
        return false;
    }
    WY_DEBUG_FMT("Setup cxx_camera with PointCloud - %d from '%d'", ret, devid);
    
    is_first = true;
    return true;
}

void MySerialPreview::consumerStop()
{
    MYDebug() ;
    is_working = false;
}

void MySerialPreview::consumerStart(wy6005param_t *para24)
{
    MYDebug() ;
    is_working = true;
    is_first = true;
    wy6005param = para24;
    wy6005_init_colortable(para24);
}

/// @brief parse the received data
/// @param packArray The data that to be parsed this time (which may include the remaining data left from last time)
/// @param left The rest, not a whole package of data
void MySerialPreview::dataArrived(QByteArray &packArray, QByteArray &partialdata)
{
    if (!is_working) return;
    if (packArray.length() <= 3) return; // Minimum length check

    int data_len, canfd_data_size, byteindex;
    uint8_t dlc;
    char *canfdheaddata;

    byteindex = 0;
    while (byteindex < packArray.length()) { // If the package is stuck, it should be subcontracted
        QByteArray canfdhead = packArray.mid(byteindex, 3); // canfd head，3 bytes
        canfdheaddata = canfdhead.data();
        dlc = canfdhead[0] & 0x0f; // canfd data body

        data_len = get_data_len(dlc); // only the specific length
        if (data_len == 0) {
            break;
        }

        QByteArray canfddata = packArray.mid(byteindex + CANFD_HEAD_SIZE, data_len);
        if (canfddata.length() == data_len) { 
            // is a complete data body
            appendPack(canfddata, data_len, currentData);
        } else { 
            // Not a complete canfd data body, Keep this part of the data, and merge the two parts of the data the next time it arrives
            partialdata = packArray.mid(byteindex); // Return the remaining data
            qDebug() << "left(" << partialdata.length() << ") -" << partialdata.toHex(' ');
            break;
        }
        canfd_data_size = data_len + CANFD_HEAD_SIZE;
        byteindex += canfd_data_size;
    }

    WRITE_BUFF("logs/serial-rawdata.txt", currentData.rawdata, WIDTH, HEIGHT*2, sizeof(uint16_t));
}

/// @brief Process a complete canfd data body
/// @param canfddata canfd data body
/// @param data_len canfd data body length
/// @param frameData Cache the data in canfd body
/// @return 
int MySerialPreview::appendPack(QByteArray & canfddata, int data_len, FrameData &frameData)
{
    int pure_data_size, recv_block_count;
    uint8_t *canfd_data, *pure_data;
    uint32_t frame_seq;
    uint8_t row, row_seq;

    canfd_data = (uint8_t *)canfddata.data();
    pure_data = canfd_data + BLOCK_HEAD_SIZE;  //Frame number (4 bytes) + Line number (1 byte) + Interline number (1 byte)
    frame_seq = *(uint32_t *)(canfd_data);
    row = canfd_data[4];
    row_seq = canfd_data[5];
    pure_data_size = data_len - BLOCK_HEAD_SIZE;

    // New frame, reset cache data
    if (frame_seq != frameData.frame) {
        frameData.renew(frame_seq);
    }
    // change to index base 0
    row--; 
    row_seq--;

    // The canfd frame length is fixed, and the data of the last frame may not fully fill. 
    // Calculate the actual data length based on the data structure
    if (row_seq == ROW_LAST_BLOCK) { 
        pure_data_size = RAW_ROW_SIZE - (ROW_LAST_BLOCK)*BLOCK_MAX_DATA_SIZE;
    }
    qDebug() << "pack " << frame_seq << "-" << row << "-" << row_seq << ":" << pure_data_size;

    // Cache the data
    uint8_t *row_data = frameData.rawdata + row * RAW_ROW_SIZE;
    uint8_t *block_data = row_data + row_seq * BLOCK_MAX_DATA_SIZE;
    memcpy(block_data, pure_data, pure_data_size); 

    // Mark the received data
    frameData.rows[row] = 1;
    frameData.block_count++;
    frameData.blocks[row * ROW_BLOCK_COUNT + row_seq] = 1;

    // Check whether a complete frame has been received
    recv_block_count = -1;
    if (frameData.isCompleted(&recv_block_count)) {
        displayFrame(frameData);
        frameData.frame = -1;
    }

    return 0;
}


void MySerialPreview::displayFrame(FrameData& framedata)
{
    qDebug() << "-----------------------------------";
    MYDebug();
    
    uint16_t distvalue[WIDTH * HEIGHT] = {0};
    uint16_t grayvalue[WIDTH * HEIGHT] = {0};

    uint8_t *distdata, *graydata;
    uint8_t *canfddata = framedata.rawdata;
    int row_size = WIDTH * 2;
    distdata = (uint8_t *)distvalue;
    graydata = (uint8_t *)grayvalue;
    for (int i = 0; i < HEIGHT; i++)
    {
        memcpy(distdata, canfddata, row_size);
        distdata += row_size;
        canfddata += row_size;

        memcpy(graydata, canfddata, row_size);
        graydata += row_size;
        canfddata += row_size;
    }
    // convert dist to pointcloud
    cv::Mat depthmat = cv::Mat::zeros(HEIGHT, WIDTH, CV_8UC3);
    cv::Mat graymat = cv::Mat::zeros(HEIGHT, WIDTH, CV_8UC3);
    WRITE_BUFF("logs/serial-distvalue.txt", distvalue, WIDTH, HEIGHT, sizeof(uint16_t));
    WRITE_BUFF("logs/serial-grayvalue.txt", grayvalue, WIDTH, HEIGHT, sizeof(uint16_t));

    wy6005_convert_dist_to_pointcloud(distvalue, *cloud_ptr, depthmat.data, wy6005param);
    // convert grayvalue to grayimg
    wy6005_convert_uint16_to_opencv(grayvalue, WIDTH, HEIGHT, graymat);

    CvtBox box;
    box.width = WIDTH;
    box.height = HEIGHT;
    box.pcd = *cloud_ptr;
    box.seq = framedata.frame;
    box.data = NULL;
    showBoxToUI(box);
    showDepthToUI(depthmat);
    showRGBToUI(graymat);
}


bool FrameData::isCompleted(int *block_count)
{
    int row, block, sum1;

    sum1 = 0;
    for (row = 0; row < HEIGHT; row++) {
        sum1 += rows[row];
    }
    if (sum1 != HEIGHT)
        return false;

    sum1 = 0;
    for (block = 0; block < FRAME_BLOCK_COUNT; block++) {
        sum1 += blocks[block];
    }
    *block_count = sum1;

    return sum1 == FRAME_BLOCK_COUNT;
}

void FrameData::renew(int f)
{
    frame = f;
    block_count = 0;
    memset(rows, 0, sizeof(rows));
    memset(blocks, 0, sizeof(blocks));
    memset(rawdata, 0, sizeof(rawdata));
}

static int get_data_len(char dlc)
{
    int data_len = 0;
    switch (dlc) {
    case 0xf:
        data_len = 64;
        break;
    case 0xd:
        data_len = 32;
        break;
    default:
        qDebug() << "invalid data length code" << QString().sprintf("0x%02X", dlc);
        break;
    }
    return data_len;
}
